#include "Function.hpp"

float Function::pointDist(cv::Point2f a, cv::Point2f b)
{
    return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}
void Function::findlight(cv::Mat frame)
{
    LightMatch mch;
    mch.setEnemyColor(false); //true :blue
    mch.saveImg(frame);
    mch.findPossible();
    lights = mch.possibles;
    mch.drawLights(frame);
    //own part
    int lights_num = lights.size();
    aim_deps::Light left, right;
    for (int i = 0; i < lights_num; i++)
    {
        for (int j = i + 1; j < lights_num; j++)
        {
            left = lights[i];
            right = lights[j];
            float angle_diff = abs(left.box.angle - right.box.angle);
            float length_ratio = (left.box.length - right.box.length) / (left.box.length ? right.box.length : left.box.length > right.box.length);

            float center_dist = pointDist(left.box.center, right.box.center);
            float length_ave = (left.box.length + right.box.length) / 2;
            float yDiff = abs(left.box.center.y - right.box.center.y);
            float xDiff = abs(left.box.center.x - right.box.center.x);
            float yDiff_ratio = yDiff / length_ave;
            float xDiff_ratio = xDiff / length_ave;
            float ratio = center_dist / length_ave;
            if (ratio > 6)
                continue;
            if (yDiff_ratio > 0.4)
                continue;
            if (xDiff_ratio < 1.4)
                continue;

            if (ratio < 0.5)
                continue;
            if (angle_diff > 5.5 || length_ratio > 0.5)
                continue;
            //
            //if(k_ratio>0.1)continue;
            //
            //
            //using center dist
            cv::Point2f center_real((left.box.center.x + right.box.center.x) / 2, (left.box.center.y + right.box.center.y) / 2);
            cv::Point2f center_predict01((left.box.vex[0].x + right.box.vex[1].x) / 2, (left.box.vex[0].y + right.box.vex[1].y) / 2);
            cv::Point2f center_predict02((left.box.vex[1].x + right.box.vex[0].x) / 2, (left.box.vex[1].y + right.box.vex[0].y) / 2);
            if (pointDist(center_real, center_predict01) > 4)
                continue;
            if (pointDist(center_real, center_predict02) > 4)
                continue;
            //draw lines
            cv::Scalar color = cv::Scalar(0, 255, 0);
            cv::line(frame, left.box.vex[0], right.box.vex[0], color, 2, cv::LINE_8);
            cv::line(frame, left.box.vex[1], right.box.vex[1], color, 2, cv::LINE_8);
            cv::circle(frame, center_real, 2, cv::Scalar(255, 0, 0), 3);
        }
    }
}